﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Coding4Fun.Kinect.Wpf;

namespace HandZooming
{
    /// <summary>
    /// MainWindow.xaml 的交互逻辑
    /// </summary>
    public partial class MainWindow : Window
    {
        public MainWindow()
        {
            InitializeComponent();
        }

        const int skeletonCount = 6;
        Skeleton[] allSkeletons = new Skeleton[skeletonCount];
        const float MaxDepthDistance = 4095; // max value returned
        const float MinDepthDistance = 850; // min value returned
        const float MaxDepthDistanceOffset = MaxDepthDistance - MinDepthDistance;
        KinectSensor _sensor;
        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            if (KinectSensor.KinectSensors.Count > 0)
            {
                _sensor = KinectSensor.KinectSensors[0];
                if (_sensor.Status == KinectStatus.Connected)
                {

                    var parameters = new TransformSmoothParameters
                    {
                        Smoothing = 0.3f,
                        Correction = 0.0f,
                        Prediction = 0.0f,
                        JitterRadius = 1.0f,
                        MaxDeviationRadius = 0.5f
                    };
                    //_sensor.SkeletonStream.Enable(parameters);
                    _sensor.SkeletonStream.Enable();
                    _sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                    _sensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);
                    
                    _sensor.ColorFrameReady += new EventHandler<ColorImageFrameReadyEventArgs>(_sensor_ColorFrameReady);
                    _sensor.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(_sensor_DepthFrameReady);
                    //_sensor.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(_sensor_SkeletonFrameReady);
                    _sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(_sensor_AllFramesReady);
                    _sensor.Start();
                    slider_angle.Value = _sensor.ElevationAngle;
                }
            }
        }

        void _sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {
            using(SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame == null)
                {
                    return;
                }
                skeletonFrame.CopySkeletonDataTo(allSkeletons);

                //Skeleton first = (from s in allSkeletons where s.TrackingState == SkeletonTrackingState.Tracked select s).FirstOrDefault();
                Skeleton first = (from s in allSkeletons
                                  where s.TrackingState == SkeletonTrackingState.Tracked
                                  select s).FirstOrDefault();
                if (first == null)
                {
                    return;
                }
                GetCameraPoint(first, e);
            }
        }
        
        private void GetCameraPoint(Skeleton first, AllFramesReadyEventArgs e)
        {
            using (DepthImageFrame depth = e.OpenDepthImageFrame())
            {
                if (depth == null || _sensor == null)
                {
                    return;
                }

//                 DepthImagePoint headDepthPoint =
//                     depth.MapFromSkeletonPoint(first.Joints[JointType.Head].Position);

                // left hand
                DepthImagePoint leftDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.HandLeft].Position);

                // right hand
                DepthImagePoint rightDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.HandRight].Position);
                
                //ColorImagePoint headColorPoint =
                //    depth.MapToColorImagePoint(headDepthPoint.X, headDepthPoint.Y,
                //    ColorImageFormat.RgbResolution640x480Fps30);
                // left hand
                ColorImagePoint leftColorPoint =
                    depth.MapToColorImagePoint(leftDepthPoint.X, leftDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                // right hand
                ColorImagePoint rightColorPoint =
                    depth.MapToColorImagePoint(rightDepthPoint.X, rightDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                                
                // set location
                // CameraPosition(headImage, headColorPoint);
                //CameraPosition(leftHandTag, leftColorPoint);
                //CameraPosition(rightHandTag, rightColorPoint);
                                
                // ScalePosition(head, first.Joints[JointType.Head]);
                ScalePosition(leftHandTag, first.Joints[JointType.HandLeft]);
                ScalePosition(rightHandTag, first.Joints[JointType.HandRight]);

                // 转化坐标
                Point ptLeft = new Point(0, 0);
                Point ptRight = new Point(0, 0);
                ptLeft.X = Canvas.GetLeft(leftHandTag) + leftHandTag.Width / 2;
                ptLeft.Y = Canvas.GetTop(leftHandTag) + leftHandTag.Height / 2 ;
                ptRight.X = Canvas.GetLeft(rightHandTag) + rightHandTag.Width / 2;
                ptRight.Y = Canvas.GetTop(rightHandTag) + rightHandTag.Height / 2 ;
                double diffRightHand = GetHandDiff(first.Joints[JointType.HandRight], first.Joints[JointType.Head]);
                double diffLeftHand = GetHandDiff(first.Joints[JointType.HandLeft], first.Joints[JointType.Head]);
                txt_handZ.Text = diffRightHand.ToString();
                if (diffLeftHand > DIFF && diffRightHand > DIFF)
                {
                    isHandIn = false;
                    Zooming(ptLeft, ptRight);
                }
                else if (diffRightHand > DIFF)
                {
                    if (isHandIn == false)
                    {
                        ptHandIn = ptRight;
                        ptPicIn.X = Canvas.GetLeft(img_zoom);
                        ptPicIn.Y = Canvas.GetTop(img_zoom);
                        isHandIn = true;
                    }
                    else
                    {
                        Draging(ptPicIn, ptHandIn, ptRight);
                    }
                }
                else
                {
                    isHandIn = false;
                }
                
            }
        }
        Point ptHandIn = new Point(0, 0);
        Point ptPicIn = new Point(0, 0);
        const double DIFF = 0.45;
        bool isHandIn = false;
        private double GetHandDiff(Joint hand, Joint head)
        {
            return (head.Position.Z - hand.Position.Z);
        }
        private void Draging(Point picStart, Point handStart, Point cur)
        {
            double imgCurX = cur.X - handStart.X + picStart.X;
            double imgCurY = cur.Y - handStart.Y + picStart.Y;
            Canvas.SetLeft(img_zoom, imgCurX);
            Canvas.SetTop(img_zoom, imgCurY);
        }
        private void Zooming(Point ptLeft, Point ptRight)
        {
            if (ptLeft.X <= 0 || ptLeft.Y <= 0 
                || ptRight.X <= 0 || ptRight.Y <= 0
                || ptRight.Y - ptLeft.Y <= 0
                || ptRight.X - ptLeft.X <= 0)
            {
                return;
            }
            Canvas.SetLeft(img_zoom, ptLeft.X); // 设置左上角坐标 
            Canvas.SetTop(img_zoom, ptLeft.Y);
            img_zoom.Height = ptRight.Y - ptLeft.Y; // 设置长宽
            img_zoom.Width = ptRight.X - ptLeft.X;
            
        }

        private void ScalePosition(FrameworkElement element, Joint joint)
        {
            //convert & scale (.3 = means 1/3 of joint distance)
            //Joint scaledJoint = joint.ScaleTo(3600, 2800, .4f, .4f);
            Joint scaledJoint = joint.ScaleTo(1920, 1080, .3f, .3f);
            Canvas.SetLeft(element, scaledJoint.Position.X);
            Canvas.SetTop(element, scaledJoint.Position.Y);
        }

        private void CameraPosition(FrameworkElement element, ColorImagePoint point)
        {
            Canvas.SetLeft(element, point.X - element.Width / 2);
            Canvas.SetTop(element, point.Y - element.Height / 2);
        }


        void _sensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            //throw new NotImplementedException();
             using(DepthImageFrame depthFrame = e.OpenDepthImageFrame())
             {
                 if (depthFrame == null)
                 {
                     return;
                 }
                 byte[] pixels = GenerateColoredBytes(depthFrame);
                 int stride = depthFrame.Width * 4;
                 img_depth.Source = BitmapSource.Create(depthFrame.Width, depthFrame.Height,
                     96, 96, PixelFormats.Bgr32, null, pixels, stride); 
             }
        }

        void _sensor_ColorFrameReady(object sender, ColorImageFrameReadyEventArgs e)
        {
            using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
            {
                if (colorFrame == null)
                {
                    return;
                }
                byte[] pixels = new byte[colorFrame.PixelDataLength];
                colorFrame.CopyPixelDataTo(pixels);
                int stride = colorFrame.Width * 4;
                img_color.Source = BitmapSource.Create(colorFrame.Width, colorFrame.Height,
                    96, 96, PixelFormats.Bgr32, null, pixels, stride);
            }
        }

        void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                sensor.Stop();
                if (sensor.AudioSource != null)
                {
                    sensor.AudioSource.Stop();
                }                
            }
        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            StopKinect(_sensor);
        }


        private byte[] GenerateColoredBytes(DepthImageFrame depthFrame)
        {
            short[] rawDepthData = new short[depthFrame.PixelDataLength];
            depthFrame.CopyPixelDataTo(rawDepthData);
            byte[] pixels = new byte[depthFrame.Width * depthFrame.Height * 4];
            const int BlueIndex = 0;
            const int GreenIndex = 1;
            const int RedIndex = 2;
            for (int depthIndex = 0, colorIndex = 0;
                depthIndex < rawDepthData.Length && colorIndex < pixels.Length;
                ++depthIndex, colorIndex += 4)
            {
                int player = rawDepthData[depthIndex] & DepthImageFrame.PlayerIndexBitmask;
                int depth = rawDepthData[depthIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth;
                if (depth <= 900)
                {
                    pixels[colorIndex + BlueIndex] = 255;
                    pixels[colorIndex + GreenIndex] = 0;
                    pixels[colorIndex + RedIndex] = 0;
                }
                else if (depth > 900 && depth < 2000)
                {
                    pixels[colorIndex + BlueIndex] = 0;
                    pixels[colorIndex + GreenIndex] = 255;
                    pixels[colorIndex + RedIndex] = 0;
                }
                else if (depth > 2000)
                {
                    pixels[colorIndex + BlueIndex] = 0;
                    pixels[colorIndex + GreenIndex] = 0;
                    pixels[colorIndex + RedIndex] = 255;
                }
                byte intensity = CalculateIntensityFromeDepth(depth);
                pixels[colorIndex + BlueIndex] = intensity;
                pixels[colorIndex + GreenIndex] = intensity;
                pixels[colorIndex + RedIndex] = intensity;

                if (player > 0)
                {
                    pixels[colorIndex + BlueIndex] = Colors.Gold.B;
                    pixels[colorIndex + GreenIndex] = Colors.Gold.G;
                    pixels[colorIndex + RedIndex] = Colors.Gold.R;
                }
            }


            return pixels;
        }


        public static byte CalculateIntensityFromeDepth(int distance)
        {
            return (byte)(255 - (255 * Math.Max(distance - MinDepthDistance, 0) / (MaxDepthDistanceOffset)));
        }

        private void slider_angle_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
        {
            if (slider_angle.Value != Convert.ToInt32(slider_angle.Value))
            {
                slider_angle.Value = Convert.ToInt32(slider_angle.Value);
            }
            label_angle.Content = slider_angle.Value.ToString();
        }

        private void slider_angle_Loaded(object sender, RoutedEventArgs e)
        {
            label_angle.Content = slider_angle.Value.ToString();
        }

        private void btn_setTilt_Click(object sender, RoutedEventArgs e)
        {
            _sensor.ElevationAngle = Convert.ToInt32(slider_angle.Value);
        }

        private void btn_reset_Click(object sender, RoutedEventArgs e)
        {
            slider_angle.Value = 0;
        }
    }
}
